#include "sdBad.h"

void sdBad::follow() {
    if(target_ != NULL) {
        turn(target_->getPos());
        move(cpv(0, 1));
    }
}

void sdBad::getChunks(std::set<sdTimed*>* chunks) {
    int count = 25;
    cpFloat div = 2 * 3.14159 / count;
    
    for(int i = 0; i < count; i++) {
        cpBody* body = new cpBody();
    
        sdTimed* bullet = new sdTimed(0.25, body);
		bullet->start();
        bullet->setMass(5.0);
    	bullet->setPos(cpv(body_->p.x + (25 * sin(-body_->a)), body_->p.y + (25 * cos(-body_->a))));
    	bullet->addCircleShape(0, 4, cpvzero, 2);

    	bullet->setVel(cpv(1000 * sin(i * div), 1000 * cos(i * div)));
 	
    	chunks->insert(bullet);
    }
}

void sdBad::move(const cpVect& direction) {
	cpFloat angle = body_->a;
	
	//TODO: CHANGE ALL SIN() COS() TO A TABLE FOR FASTER SPEED
	cpFloat c = cos(angle);
	cpFloat s = -sin(angle);
	
	cpVect force = cpv(0, 0);
	if(direction.x < 0) {
		force.x += direction.x * leftForce_ * c;
		force.y += -direction.x * leftForce_ * s;
	} else {
		force.x += direction.x * rightForce_ * c;
		force.y += -direction.x * rightForce_ * s;
	}
	
	if(direction.y < 0) {
		force.x += direction.y * reverseForce_ * s;
		force.y += direction.y * reverseForce_ * c;
	} else {
	    force.x += direction.y * forwardsForce_ * s;
		force.y += direction.y * forwardsForce_ * c;
	}
	
	body_->f = force;
}

void sdBad::turn(const cpVect& direction) {
	cpFloat shipToMouseAngle = -cpvtoangle(cpvsub(direction, body_->p));
	cpFloat turn = (shipToMouseAngle - body_->a);
	
    if(turn > 3.14159) {
        turn -= (2 * 3.14159);
    } else if(turn < -3.14159) {
        turn += (2 * 3.14159);
    }
    
    turn *= deltaW_;

	body_->w = turn;
}
